#!/usr/bin/env python

import roslib; roslib.load_manifest('team4')
import rospy
from lift_test import Tray 
from geometry_msgs.msg import PointStamped
from ObjectUnstacker import *
from ObjectStacker import *
from PlatePicker import *
from Room3 import *
from free_table_spot import *
from pr2_python import base, head, gripper, world_interface
#from filename_of_your_module import * <- does not need the .py. Should import classes, functions, and maybe global vars

# Before calling import on your file, make sure of three things:
   # if it is team4/ then it can stay in scripts/ or be in src/
   # if it is in stacked_objects/ it has to be in src/
   # there cannot be an init_node in your code. If your code needs that to run, then you need an interface of some sort and we need to launch it seperately

# If there is anything that your code needs launched in addition to the usual, add it to the demo.launch file.

# Really, this should be done as a state machine. Just pretend it's one for now.

def part1():
    tray = Tray()
    base_mover = base.Base()
    stacker = ObjectStacker()
    picker = PlatePicker()
    room3 = Room3()
    ou = ObjectUnstacker()

    gr_r = gripper.Gripper('right_arm')
    gr_r.open()
    gr_l = gripper.Gripper('left_arm')
    gr_l.open()
    
    ou.reset_everything()
    stacker.move_arms_back()
    h=head.Head()
    h.look_at_relative_point(0.5, 0.0, 0.75)
    ou.reset_everything()
    rospy.loginfo("Going to dirty table to pick up tray. ")
    # Go to dirty table
    print "Press key when you're ready to move"
    raw_input()
    room3.goto_dirty_table()
    print "Press key when you're ready to pickup tray"
    raw_input()

    tray.pick_up_tray()
    
    rospy.loginfo("Going back to clean table")
    # go to clean table
    print "Press key when you're ready to move"
    #raw_input()
    base_mover.move_to(-0.76, 0.32, -3.08)
#    room3.move_to_table()
    
#    b = base.Base()
#    currentPose = b.get_current_pose()
#    b.move_to(currentPose[0], currentPose[1], currentPose[2]+1.51)
#    h = head.Head()
#    h.look_at_map_point(-2.0, 0.2, 0.75)
    
    #room3.move_to_table()
    
#    rospy.loginfo("Scanning table to find all objects and free space")
#    print "Press key to move tray down"
#    raw_input()
#    tray.get_tray_out_of_the_way()
#    PointStamped()
#    # scan area
#    look_at_p = PointStamped()
#    look_at_p.header.frame_id = '/map'
#    look_at_p.point.x = -2.0
#    look_at_p.point.y = 0.2
#    look_at_p.point.z = 0.75
    #center_free_spot=get_free_spot(width=0.4,length=0.6,p=look_at_p)
    #h.look_at_relative_point(0.8, 0.0, 0.8)
    
    #base.move_manipulable_pose(center_free_spot.pose.position.x,center_free_spot.pose.position.y,center_free_spot.pose.position.z)
    
    # move to robot in front of freespace on table
    rospy.loginfo("Putting tray down")
    print "Press key to move tray down"
    raw_input()
    p = PoseStamped()
    p.pose.position.x = 0.55
    p.pose.position.y = 0.0
    p.pose.position.z = 0.65
    p.pose.orientation.w = 1.0
    p.header.frame_id = '/base_link'
    tray.put_down_tray(p)
    
    stacker.move_arms_back()
    ou.reset_everything()
    
    # PART 1 DONE 

def part2():
    tray = Tray()
    base_mover = base.Base()
    stacker = ObjectStacker()
    picker = PlatePicker()
    room3 = Room3()
    ou = ObjectUnstacker()

    gr_r = gripper.Gripper('right_arm')
    gr_r.open()
    gr_l = gripper.Gripper('left_arm')
    gr_l.open()
    
    p = PoseStamped()
    p.pose.position.x = 1.0
    p.pose.position.y = -0.5
    p.pose.position.z = 2.1
    p.pose.orientation.w = 1.0
    p.header.frame_id = '/map'

    print "Press key to move to stacking area"
    raw_input()
    base_mover.move_manipulable_pose(-1.7, 0.5, 0.75)

    #center_free_spot = PoseStamped()
    #center_free_spot.header.frame_id = "/map"
    #center_free_spot.pose = transform_listener.transform_pose(center_free_spot.header.frame_id, p.header.frame_id, p.pose )       

    rospy.loginfo("Stacking items on tray")
    # stacking objects code
    print "Press key to stack objects"
    raw_input()
    stacker.stack_objects()
    #or
    #stacker.pose_stack_objects(p)
    
    stacker.move_arms_back()
    ou.reset_everything()
   
    # PART 2 DONE
     
def part3():
    tray = Tray()
    base_mover = base.Base()
    stacker = ObjectStacker()
    picker = PlatePicker()
    room3 = Room3()
    ou = ObjectUnstacker()

    gr_r = gripper.Gripper('right_arm')
    gr_r.open()
    gr_l = gripper.Gripper('left_arm')
    gr_l.open()
    
    rospy.loginfo("Moving to tray")
    print "Press key to move to tray"
    raw_input()
    b.move_to(-0.76, 0.32, -3.08)
    
    rospy.loginfo("Picking up tray")
    print "Press key to pick up tray"
    raw_input()
    tray.pick_up_tray() # ignores middle of tray, so please make sure all objects are in middle
    
    rospy.loginfo("Going to dirty table to leave tray")
    print "Press key to move"
    raw_input()
    
    room3.move_to_dirty_table()
    print "Press key to put down tray"
    p = PoseStamped()
    p.pose.position.x = 0.55
    p.pose.position.y = 0.0
    p.pose.position.z = 0.65
    p.pose.orientation.w = 1.0
    p.header.frame_id = '/base_link'
    tray.put_down_tray(p)

    stacker.move_arms_back()
    ou.reset_everything()
    
    # PART 3 DONE 

def part4():
    tray = Tray()
    base_mover = base.Base()
    stacker = ObjectStacker()
    picker = PlatePicker()
    room3 = Room3()
    ou = ObjectUnstacker()

    gr_r = gripper.Gripper('right_arm')
    gr_r.open()
    gr_l = gripper.Gripper('left_arm')
    gr_l.open()
    
    rospy.loginfo("Picking up stack of plates")
    # pickup plate stack code
    print "Press key to pick up stack"
    raw_input()
    #picker.pick_plate_stack()
    print "Press key to move"
    raw_input()
    #room3.goto_table()
    print "Press key to drop on table"
    raw_input()
    #picker.drop_stack_on_table()
    
    rospy.loginfo("Unstacking tableware")
    # unstacking plates code
    print "Press key to unstack"
    raw_input()
    #picker.put_plate_from_stack_on_table()

    # unstacking cups
    print "Press key to move"
    raw_input()
    rospy.loginfo("Picking up and placing stack of cups")
    ou.unstack_objects()

if __name__ == "__main__":
   rospy.init_node("team4_demo")
   
   rospy.loginfo("Beginning team 4 demo")
   rospy.loginfo("Creating all class instances")
   # instantiate classes here (if they need instantiating)
   
   part1()
   #part2()
   #part3()
   #part4()
   

